#include <iostream>
#include <cmath>
#include <cstdlib>
#include <vector>
#include <cmath>
#include <algorithm>
#include <iomanip>
#include <complex>
#include <numeric>

// 复数类型定义（用于基带信号处理）
using Complex = std::complex<float>;

// 雷达系统参数
struct RadarParams {
    float carrier_freq;    // 载频 (Hz)
    float prf;             // 脉冲重复频率 (Hz)
    int num_pulses;        // 相干处理脉冲数
    float range_res;       // 距离分辨率 (m)
    float max_range;       // 最大探测距离 (m)
    float doppler_res;     // 多普勒分辨率 (m/s)
};

// 雷达原始回波数据（慢时间-距离维度）
struct RadarRawData {
    std::vector<std::vector<Complex>> data;  // [脉冲数][距离门]
    int num_range_bins;                      // 距离门数量
    int num_pulses;                          // 脉冲数
};

// 处理后的数据（距离-多普勒谱）
struct ProcessedData {
    std::vector<std::vector<float>> power;   // 功率谱 [距离门][多普勒通道]
    int num_range_bins;                      // 距离门数量
    int num_doppler_bins;                    // 多普勒通道数量
    RadarParams radar_params;                // 雷达参数
};

// 检测到的目标点迹
struct TargetPoint {
    float range;           // 距离 (m)
    float doppler_vel;     // 多普勒速度 (m/s)
    float power;           // 目标功率
    int range_bin;         // 距离门索引
    int doppler_bin;       // 多普勒通道索引
};

// CFAR检测参数
struct CFARParams {
    int guard_range;       // 距离方向保护单元
    int guard_doppler;     // 多普勒方向保护单元
    int ref_range;         // 距离方向参考单元
    int ref_doppler;       // 多普勒方向参考单元
    float pfa;             // 虚警概率
};

// MTI/MTD处理器
class MtiMtdProcessor {
private:
    RadarParams params;
    std::vector<float> mti_coeffs;  // MTI滤波器系数（高通）

public:
    MtiMtdProcessor(RadarParams radar_params) : params(radar_params) {
        // 初始化MTI滤波器（双脉冲对消器）
        mti_coeffs = {1.0f, -1.0f};  // 简单一阶差分滤波器
    }

    // MTI处理：抑制固定杂波（对慢时间维度滤波）
    RadarRawData mtiProcess(const RadarRawData& raw_data) {
        RadarRawData mti_data = raw_data;
        int num_pulses = raw_data.num_pulses;
        int num_ranges = raw_data.num_range_bins;

        // 对每个距离门应用MTI滤波器
        for (int r = 0; r < num_ranges; r++) {
            for (int p = mti_coeffs.size() - 1; p < num_pulses; p++) {
                Complex sum(0, 0);
                for (int i = 0; i < mti_coeffs.size(); i++) {
                    sum += mti_coeffs[i] * raw_data.data[p - i][r];
                }
                mti_data.data[p][r] = sum;
            }
        }
        return mti_data;
    }

    // MTD处理：多普勒滤波（FFT实现）
    ProcessedData mtdProcess(const RadarRawData& mti_data) {
        ProcessedData mtd_data;
        mtd_data.num_range_bins = mti_data.num_range_bins;
        mtd_data.num_doppler_bins = params.num_pulses;
        mtd_data.radar_params = params;
        mtd_data.power.resize(mtd_data.num_range_bins, 
                            std::vector<float>(mtd_data.num_doppler_bins, 0.0f));

        // 对每个距离门进行FFT（慢时间维度）
        for (int r = 0; r < mtd_data.num_range_bins; r++) {
            // 提取慢时间序列
            std::vector<Complex> slow_time(mti_data.num_pulses);
            for (int p = 0; p < mti_data.num_pulses; p++) {
                slow_time[p] = mti_data.data[p][r];
            }

            // FFT计算多普勒谱
            fft(slow_time);

            // 计算功率谱（模值平方）
            for (int d = 0; d < mtd_data.num_doppler_bins; d++) {
                mtd_data.power[r][d] = std::norm(slow_time[d]);
            }
        }

        return mtd_data;
    }

private:
    // 快速傅里叶变换（简化实现，实际可使用FFTW库）
    void fft(std::vector<Complex>& x) {
        int n = x.size();
        if (n <= 1) return;

        // 分离偶数和奇数索引
        std::vector<Complex> even(n/2), odd(n/2);
        for (int i = 0; 2*i < n; i++) {
            even[i] = x[2*i];
            odd[i] = x[2*i+1];
        }

        fft(even);
        fft(odd);

        // 合并结果
        for (int i = 0; 2*i < n; i++) {
            Complex t = std::polar<float>(1.0f, -2 * M_PI * i / n) * odd[i];
            x[i] = even[i] + t;
            x[i + n/2] = even[i] - t;
        }
    }
};

// 目标检测器（CFAR+点迹凝聚）
class TargetDetector {
private:
    CFARParams cfar_params;

    // 2D CFAR检测（距离-多普勒二维窗）
    std::vector<std::pair<int, int>> cfarDetect(const ProcessedData& mtd_data) {
        std::vector<std::pair<int, int>> detections;
        int num_ranges = mtd_data.num_range_bins;
        int num_dopplers = mtd_data.num_doppler_bins;

        for (int r = 0; r < num_ranges; r++) {
            for (int d = 0; d < num_dopplers; d++) {
                // 收集参考单元
                std::vector<float> ref_powers;

                // 距离方向扩展参考窗
                for (int dr = -cfar_params.ref_range; dr <= cfar_params.ref_range; dr++) {
                    // 跳过保护单元和当前单元
                    if (abs(dr) <= cfar_params.guard_range) continue;
                    
                    int nr = r + dr;
                    if (nr < 0 || nr >= num_ranges) continue;

                    // 多普勒方向扩展参考窗
                    for (int dd = -cfar_params.ref_doppler; dd <= cfar_params.ref_doppler; dd++) {
                        if (abs(dd) <= cfar_params.guard_doppler) continue;
                        
                        int nd = d + dd;
                        if (nd < 0 || nd >= num_dopplers) continue;

                        ref_powers.push_back(mtd_data.power[nr][nd]);
                    }
                }

                // 参考单元不足时跳过
                if (ref_powers.size() < 4) continue;

                // 计算阈值（CA-CFAR）
                float avg_power = std::accumulate(ref_powers.begin(), ref_powers.end(), 0.0f) 
                                / ref_powers.size();
                float threshold = avg_power * (pow(cfar_params.pfa, -1.0f / ref_powers.size()) - 1.0f);

                // 判决
                if (mtd_data.power[r][d] > threshold) {
                    detections.emplace_back(r, d);
                }
            }
        }

        return detections;
    }

    // 点迹凝聚：合并相邻检测点
    std::vector<TargetPoint> clusterDetections(
        const std::vector<std::pair<int, int>>& detections, //[{0:2},{1:3}  ]
        const ProcessedData& mtd_data) {
        
        std::vector<TargetPoint> targets;
        std::vector<bool> visited(detections.size(), false);

        // 凝聚阈值（距离门和多普勒通道）
        const int range_cluster_thresh = 2;    // 距离方向相邻2个门内视为同一目标
        const int doppler_cluster_thresh = 2;  // 多普勒方向相邻2个通道内视为同一目标

        for (int i = 0; i < detections.size(); i++) {
            if (visited[i]) continue;

            // 初始点
            int r0 = detections[i].first;
            int d0 = detections[i].second;
            std::vector<std::pair<int, int>> cluster;
            cluster.push_back(detections[i]);
            visited[i] = true;

            // 寻找相邻检测点
            for (int j = i + 1; j < detections.size(); j++) {
                if (visited[j]) continue;

                int r1 = detections[j].first;
                int d1 = detections[j].second;

                // 判断是否在凝聚范围内
                if (abs(r1 - r0) <= range_cluster_thresh && 
                    abs(d1 - d0) <= doppler_cluster_thresh) {
                    cluster.push_back(detections[j]);
                    visited[j] = true;
                }
            }

            // 计算聚类中心（功率加权平均）
            float sum_power = 0.0f;
            float sum_r = 0.0f, sum_d = 0.0f;
            for (const auto& p : cluster) {
                float power = mtd_data.power[p.first][p.second];
                sum_power += power;
                sum_r += p.first * power;
                sum_d += p.second * power;
            }

            int center_r = static_cast<int>(sum_r / sum_power + 0.5f);
            int center_d = static_cast<int>(sum_d / sum_power + 0.5f);

            // 转换为物理量
            TargetPoint tp;
            tp.range_bin = center_r;
            tp.doppler_bin = center_d;
            tp.range = center_r * mtd_data.radar_params.range_res;
            // 多普勒速度计算（考虑 Nyquist 折叠）
            int half_doppler = mtd_data.num_doppler_bins / 2;
            int vel_sign = (center_d <= half_doppler) ? 1 : -1;
            tp.doppler_vel = vel_sign * (center_d % half_doppler) * mtd_data.radar_params.doppler_res;
            tp.power = sum_power / cluster.size();

            targets.push_back(tp);
        }

        return targets;
    }

public:
    TargetDetector(CFARParams params) : cfar_params(params) {}

    // 完整检测流程：CFAR + 点迹凝聚
    std::vector<TargetPoint> detect(const ProcessedData& mtd_data) {
        // 1. CFAR检测
        auto detections = cfarDetect(mtd_data);
        if (detections.empty()) {
            return {};
        }

        // 2. 点迹凝聚
        return clusterDetections(detections, mtd_data);
    }

    // 打印目标点迹
    void printTargets(const std::vector<TargetPoint>& targets) {
        std::cout << "\n检测到的目标点迹（共 " << targets.size() << " 个）：" << std::endl;
        std::cout << "距离(m) | 速度(m/s) | 距离门 | 多普勒通道 | 平均功率" << std::endl;
        std::cout << "-----------------------------------------------------" << std::endl;
        for (const auto& tp : targets) {
            std::cout << std::fixed << std::setprecision(2)
                      << tp.range << "      | "
                      << tp.doppler_vel << "        | "
                      << tp.range_bin << "      | "
                      << tp.doppler_bin << "         | "
                      << tp.power << std::endl;
        }
    }
};

// 生成模拟雷达原始数据
RadarRawData generateSimulatedRawData(RadarParams params) {
    RadarRawData raw_data;
    raw_data.num_range_bins = static_cast<int>(params.max_range / params.range_res);
    raw_data.num_pulses = params.num_pulses;
    raw_data.data.resize(params.num_pulses, std::vector<Complex>(raw_data.num_range_bins, 0.0f));

    // 1. 添加固定杂波（所有脉冲相同）
    for (int r = 0; r < raw_data.num_range_bins; r++) {
        // 杂波功率随距离衰减
        float clutter_power = 100.0f / (1.0f + 0.01f * r);
        Complex clutter(clutter_power * (rand() % 100 / 100.0f), 
                        clutter_power * (rand() % 100 / 100.0f));
        for (int p = 0; p < params.num_pulses; p++) {
            raw_data.data[p][r] += clutter;
        }
    }

    // 2. 添加运动目标
    // 目标1：距离=50m，速度=10m/s，功率=200
    int target1_range_bin = static_cast<int>(50.0f / params.range_res);
    float lambda = 3e8f / params.carrier_freq;  // 波长
    float doppler_freq1 = 2 * 10.0f / lambda;   // 多普勒频率
    for (int p = 0; p < params.num_pulses; p++) {
        // 慢时间相位变化：exp(j*2π*f_d*p/PRF)
        float phase = 2 * M_PI * doppler_freq1 * p / params.prf;
        Complex target(cos(phase), sin(phase));
        raw_data.data[p][target1_range_bin] += target * 200.0f;
    }

    // 目标2：距离=80m，速度=-5m/s，功率=150
    int target2_range_bin = static_cast<int>(80.0f / params.range_res);
    float doppler_freq2 = 2 * (-5.0f) / lambda;
    for (int p = 0; p < params.num_pulses; p++) {
        float phase = 2 * M_PI * doppler_freq2 * p / params.prf;
        Complex target(cos(phase), sin(phase));
        raw_data.data[p][target2_range_bin] += target * 150.0f;
    }

    // 3. 添加噪声
    for (int p = 0; p < params.num_pulses; p++) {
        for (int r = 0; r < raw_data.num_range_bins; r++) {
            float noise_amp = 5.0f;
            Complex noise(noise_amp * (rand() % 100 / 100.0f - 0.5f),
                         noise_amp * (rand() % 100 / 100.0f - 0.5f));
            raw_data.data[p][r] += noise;
        }
    }

    return raw_data;
}

int main() {
    printf("execute");
    std::cout << "生成模拟雷达回波数据..." << std::endl;
    // 1. 配置雷达参数
    RadarParams radar_params;
    radar_params.carrier_freq = 10e9f;    // 10GHz
    radar_params.prf = 1000.0f;           // 1kHz
    radar_params.num_pulses = 32;         // 32个相干脉冲
    radar_params.range_res = 1.0f;        // 1m距离分辨率
    radar_params.max_range = 200.0f;      // 最大200m
    radar_params.doppler_res = (3e8f / radar_params.carrier_freq) * radar_params.prf 
                             / (2 * radar_params.num_pulses);  // 多普勒分辨率计算

    // 2. 配置CFAR参数
    CFARParams cfar_params;
    cfar_params.guard_range = 1;          // 距离保护单元1个
    cfar_params.guard_doppler = 1;        // 多普勒保护单元1个
    cfar_params.ref_range = 3;            // 距离参考单元3个
    cfar_params.ref_doppler = 3;          // 多普勒参考单元3个
    cfar_params.pfa = 1e-4f;              // 虚警概率0.01%

    // 3. 生成模拟数据
    std::cout << "生成模拟雷达回波数据..." << std::endl;
    RadarRawData raw_data = generateSimulatedRawData(radar_params);

    // 4. MTI+MTD处理
    std::cout << "执行MTI和MTD处理..." << std::endl;
    MtiMtdProcessor mti_mtd(radar_params);
    RadarRawData mti_data = mti_mtd.mtiProcess(raw_data);
    ProcessedData mtd_data = mti_mtd.mtdProcess(mti_data);

    // 5. 目标检测（CFAR+点迹凝聚）
    std::cout << "执行目标检测和点迹凝聚..." << std::endl;
    TargetDetector detector(cfar_params);
    std::vector<TargetPoint> targets = detector.detect(mtd_data);

    // 6. 输出结果
    detector.printTargets(targets);

    return 0;
}
